// Copyright (C) 2005, 2008 International Business Machines and others.
// All Rights Reserved.
// This code is published under the Eclipse Public License.
//
// Authors:  Carl Laird, Andreas Waechter     IBM    2005-06-25

#include "IpNLPScaling.hpp"
#include "IpSymMatrix.hpp"
#include "IpScaledMatrix.hpp"
#include "IpSymScaledMatrix.hpp"

namespace Ipopt
{

#if IPOPT_VERBOSITY > 0
static const Index dbg_verbosity = 0;
#endif

NLPScalingObject::NLPScalingObject()
{ }

NLPScalingObject::~NLPScalingObject()
{ }

SmartPtr<Vector> NLPScalingObject::apply_vector_scaling_x_LU_NonConst(
   const Matrix&                 Px_LU,
   const SmartPtr<const Vector>& lu,
   const VectorSpace&            x_space
)
{
   DBG_START_METH("NLPScalingObject::apply_vector_scaling_x_LU_NonConst", dbg_verbosity);
   SmartPtr<Vector> scaled_x_LU = lu->MakeNew();
   if( have_x_scaling() )
   {
      SmartPtr<Vector> tmp_x = x_space.MakeNew();

      // move to full x space
      Px_LU.MultVector(1.0, *lu, 0.0, *tmp_x);

      // scale in full x space
      tmp_x = apply_vector_scaling_x_NonConst(ConstPtr(tmp_x));

      // move back to x_L space
      Px_LU.TransMultVector(1.0, *tmp_x, 0.0, *scaled_x_LU);
   }
   else
   {
      scaled_x_LU->Copy(*lu);
   }

   return scaled_x_LU;
}

SmartPtr<const Vector> NLPScalingObject::apply_vector_scaling_x_LU(
   const Matrix&                 Px_LU,
   const SmartPtr<const Vector>& lu,
   const VectorSpace&            x_space
)
{
   DBG_START_METH("NLPScalingObject::apply_vector_scaling_x_LU", dbg_verbosity);
   if( have_x_scaling() )
   {
      return ConstPtr(apply_vector_scaling_x_LU_NonConst(Px_LU, lu, x_space));
   }
   else
   {
      return lu;
   }
}

SmartPtr<Vector> NLPScalingObject::apply_vector_scaling_d_LU_NonConst(
   const Matrix&                 Pd_LU,
   const SmartPtr<const Vector>& lu,
   const VectorSpace&            d_space
)
{
   DBG_START_METH("NLPScalingObject::apply_vector_scaling_d_LU_NonConst", dbg_verbosity);
   SmartPtr<Vector> scaled_d_LU = lu->MakeNew();
   if( have_d_scaling() )
   {
      SmartPtr<Vector> tmp_d = d_space.MakeNew();

      // move to full d space
      Pd_LU.MultVector(1.0, *lu, 0.0, *tmp_d);

      // scale in full x space
      tmp_d = apply_vector_scaling_d_NonConst(ConstPtr(tmp_d));

      // move back to x_L space
      Pd_LU.TransMultVector(1.0, *tmp_d, 0.0, *scaled_d_LU);
   }
   else
   {
      scaled_d_LU->Copy(*lu);
   }

   return scaled_d_LU;
}

SmartPtr<const Vector> NLPScalingObject::apply_vector_scaling_d_LU(
   const Matrix&                 Pd_LU,
   const SmartPtr<const Vector>& lu,
   const VectorSpace&            d_space
)
{
   DBG_START_METH("NLPScalingObject::apply_vector_scaling_d_LU", dbg_verbosity);
   if( have_d_scaling() )
   {
      return ConstPtr(apply_vector_scaling_d_LU_NonConst(Pd_LU, lu, d_space));
   }
   else
   {
      return lu;
   }
}

SmartPtr<Vector> NLPScalingObject::unapply_vector_scaling_d_LU_NonConst(
   const Matrix&                 Pd_LU,
   const SmartPtr<const Vector>& lu,
   const VectorSpace&            d_space
)
{
   DBG_START_METH("NLPScalingObject::unapply_vector_scaling_d_LU_NonConst", dbg_verbosity);
   SmartPtr<Vector> unscaled_d_LU = lu->MakeNew();
   if( have_d_scaling() )
   {
      SmartPtr<Vector> tmp_d = d_space.MakeNew();

      // move to full d space
      Pd_LU.MultVector(1.0, *lu, 0.0, *tmp_d);

      // scale in full x space
      tmp_d = unapply_vector_scaling_d_NonConst(ConstPtr(tmp_d));

      // move back to x_L space
      Pd_LU.TransMultVector(1.0, *tmp_d, 0.0, *unscaled_d_LU);
   }
   else
   {
      unscaled_d_LU->Copy(*lu);
   }

   return unscaled_d_LU;
}

SmartPtr<const Vector> NLPScalingObject::unapply_vector_scaling_d_LU(
   const Matrix&                 Pd_LU,
   const SmartPtr<const Vector>& lu,
   const VectorSpace&            d_space
)
{
   DBG_START_METH("NLPScalingObject::unapply_vector_scaling_d_LU", dbg_verbosity);
   if( have_d_scaling() )
   {
      return ConstPtr(unapply_vector_scaling_d_LU_NonConst(Pd_LU, lu, d_space));
   }
   else
   {
      return lu;
   }
}

SmartPtr<Vector> NLPScalingObject::apply_grad_obj_scaling_NonConst(
   const SmartPtr<const Vector>& v
)
{
   DBG_START_METH("NLPScalingObject::apply_grad_obj_scaling_NonConst", dbg_verbosity);
   SmartPtr<Vector> scaled_v = unapply_vector_scaling_x_NonConst(v);
   Number df = apply_obj_scaling(1.0);
   if( df != 1. )
   {
      scaled_v->Scal(df);
   }
   return scaled_v;
}

SmartPtr<const Vector> NLPScalingObject::apply_grad_obj_scaling(
   const SmartPtr<const Vector>& v
)
{
   DBG_START_METH("NLPScalingObject::apply_grad_obj_scaling", dbg_verbosity);
   Number df = apply_obj_scaling(1.);
   if( df != 1. )
   {
      SmartPtr<Vector> scaled_v = apply_grad_obj_scaling_NonConst(v);
      return ConstPtr(scaled_v);
   }
   else
   {
      SmartPtr<const Vector> scaled_v = unapply_vector_scaling_x(v);
      return scaled_v;
   }
}

SmartPtr<Vector> NLPScalingObject::unapply_grad_obj_scaling_NonConst(
   const SmartPtr<const Vector>& v
)
{
   DBG_START_METH("NLPScalingObject::unapply_grad_obj_scaling_NonConst", dbg_verbosity);
   SmartPtr<Vector> unscaled_v = apply_vector_scaling_x_NonConst(v);
   Number df = unapply_obj_scaling(1.);
   if( df != 1. )
   {
      unscaled_v->Scal(df);
   }
   return unscaled_v;
}

SmartPtr<const Vector> NLPScalingObject::unapply_grad_obj_scaling(
   const SmartPtr<const Vector>& v
)
{
   DBG_START_METH("NLPScalingObject::unapply_grad_obj_scaling", dbg_verbosity);
   Number df = unapply_obj_scaling(1.);
   if( df != 1. )
   {
      SmartPtr<Vector> unscaled_v = unapply_grad_obj_scaling_NonConst(v);
      return ConstPtr(unscaled_v);
   }
   else
   {
      SmartPtr<const Vector> scaled_v = apply_vector_scaling_x(v);
      return scaled_v;
   }
}

StandardScalingBase::StandardScalingBase()
{ }

StandardScalingBase::~StandardScalingBase()
{ }

void StandardScalingBase::RegisterOptions(
   SmartPtr<RegisteredOptions> roptions)
{
   roptions->AddNumberOption(
      "obj_scaling_factor",
      "Scaling factor for the objective function.",
      1.,
      "This option sets a scaling factor for the objective function. "
      "The scaling is seen internally by Ipopt but the unscaled objective is reported in the console output. "
      "If additional scaling parameters are computed (e.g. user-scaling or gradient-based), both factors are multiplied. "
      "If this value is chosen to be negative, Ipopt will maximize the objective function instead of minimizing it.");
}

bool StandardScalingBase::InitializeImpl(
   const OptionsList& options,
   const std::string& prefix
)
{
   options.GetNumericValue("obj_scaling_factor", obj_scaling_factor_, prefix);
   return true;
}

void StandardScalingBase::DetermineScaling(
   const SmartPtr<const VectorSpace>    x_space,
   const SmartPtr<const VectorSpace>    c_space,
   const SmartPtr<const VectorSpace>    d_space,
   const SmartPtr<const MatrixSpace>    jac_c_space,
   const SmartPtr<const MatrixSpace>    jac_d_space,
   const SmartPtr<const SymMatrixSpace> h_space,
   SmartPtr<const MatrixSpace>&         new_jac_c_space,
   SmartPtr<const MatrixSpace>&         new_jac_d_space,
   SmartPtr<const SymMatrixSpace>&      new_h_space,
   const Matrix&                        Px_L,
   const Vector&                        x_L,
   const Matrix&                        Px_U,
   const Vector&                        x_U
)
{
   SmartPtr<Vector> dc;
   SmartPtr<Vector> dd;
   DetermineScalingParametersImpl(x_space, c_space, d_space, jac_c_space, jac_d_space, h_space, Px_L, x_L, Px_U, x_U,
                                  df_, dx_, dc, dd);

   df_ *= obj_scaling_factor_;

   if( Jnlst().ProduceOutput(J_DETAILED, J_MAIN) )
   {
      Jnlst().Printf(J_DETAILED, J_MAIN,
                     "objective scaling factor = %g\n", df_);
      if( IsValid(dx_) )
      {
         Jnlst().Printf(J_DETAILED, J_MAIN,
                        "x scaling provided\n");
      }
      else
      {
         Jnlst().Printf(J_DETAILED, J_MAIN,
                        "No x scaling provided\n");
      }
      if( IsValid(dc) )
      {
         Jnlst().Printf(J_DETAILED, J_MAIN,
                        "c scaling provided\n");
      }
      else
      {
         Jnlst().Printf(J_DETAILED, J_MAIN,
                        "No c scaling provided\n");
      }
      if( IsValid(dd) )
      {
         Jnlst().Printf(J_DETAILED, J_MAIN,
                        "d scaling provided\n");
      }
      else
      {
         Jnlst().Printf(J_DETAILED, J_MAIN,
                        "No d scaling provided\n");
      }
   }

   if( Jnlst().ProduceOutput(J_VECTOR, J_MAIN) )
   {
      if( IsValid(dx_) )
      {
         dx_->Print(Jnlst(), J_VECTOR, J_MAIN, "x scaling vector");
      }
      if( IsValid(dc) )
      {
         dc->Print(Jnlst(), J_VECTOR, J_MAIN, "c scaling vector");
      }
      if( IsValid(dd) )
      {
         dd->Print(Jnlst(), J_VECTOR, J_MAIN, "d scaling vector");
      }
   }

   // create the scaling matrix spaces
   if( IsValid(dx_) || IsValid(dc) )
   {
      scaled_jac_c_space_ = new ScaledMatrixSpace(ConstPtr(dc), false, jac_c_space, ConstPtr(dx_), true);
      new_jac_c_space = GetRawPtr(scaled_jac_c_space_);
   }
   else
   {
      scaled_jac_c_space_ = NULL;
      new_jac_c_space = jac_c_space;
   }

   if( IsValid(dx_) || IsValid(dd) )
   {
      scaled_jac_d_space_ = new ScaledMatrixSpace(ConstPtr(dd), false, jac_d_space, ConstPtr(dx_), true);
      new_jac_d_space = GetRawPtr(scaled_jac_d_space_);
   }
   else
   {
      scaled_jac_d_space_ = NULL;
      new_jac_d_space = jac_d_space;
   }

   if( IsValid(h_space) )
   {
      if( IsValid(dx_) )
      {
         scaled_h_space_ = new SymScaledMatrixSpace(ConstPtr(dx_), true, h_space);
         new_h_space = GetRawPtr(scaled_h_space_);
      }
      else
      {
         scaled_h_space_ = NULL;
         new_h_space = h_space;
      }
   }
   else
   {
      new_h_space = NULL;
   }
}

Number StandardScalingBase::apply_obj_scaling(
   const Number& f
)
{
   DBG_START_METH("NLPScalingObject::apply_obj_scaling", dbg_verbosity);
   return df_ * f;
}

Number StandardScalingBase::unapply_obj_scaling(
   const Number& f
)
{
   DBG_START_METH("NLPScalingObject::unapply_obj_scaling", dbg_verbosity);
   return f / df_;
}

SmartPtr<Vector> StandardScalingBase::apply_vector_scaling_x_NonConst(
   const SmartPtr<const Vector>& v
)
{
   DBG_START_METH("StandardScalingBase::apply_vector_scaling_x_NonConst",
                  dbg_verbosity);
   SmartPtr<Vector> scaled_x = v->MakeNewCopy();
   if( IsValid(dx_) )
   {
      scaled_x->ElementWiseMultiply(*dx_);
   }
   else
   {
      DBG_PRINT((1, "Creating copy in apply_vector_scaling_x_NonConst!"));
   }
   return scaled_x;
}

SmartPtr<const Vector> StandardScalingBase::apply_vector_scaling_x(
   const SmartPtr<const Vector>& v
)
{
   DBG_START_METH("NLPScalingObject::apply_vector_scaling_x", dbg_verbosity);
   if( IsValid(dx_) )
   {
      return ConstPtr(apply_vector_scaling_x_NonConst(v));
   }
   else
   {
      return v;
   }
}

SmartPtr<Vector> StandardScalingBase::unapply_vector_scaling_x_NonConst(
   const SmartPtr<const Vector>& v
)
{
   DBG_START_METH("StandardScalingBase::unapply_vector_scaling_x_NonConst",
                  dbg_verbosity);
   SmartPtr<Vector> unscaled_x = v->MakeNewCopy();
   if( IsValid(dx_) )
   {
      unscaled_x->ElementWiseDivide(*dx_);
   }
   else
   {
      DBG_PRINT((1, "Creating copy in unapply_vector_scaling_x_NonConst!"));
   }
   return unscaled_x;
}

SmartPtr<const Vector> StandardScalingBase::unapply_vector_scaling_x(
   const SmartPtr<const Vector>& v
)
{
   DBG_START_METH("NLPScalingObject::unapply_vector_scaling_x", dbg_verbosity);
   if( IsValid(dx_) )
   {
      return ConstPtr(unapply_vector_scaling_x_NonConst(v));
   }
   else
   {
      return v;
   }
}

SmartPtr<Vector> StandardScalingBase::apply_vector_scaling_c_NonConst(
   const SmartPtr<const Vector>& v
)
{
   DBG_START_METH("StandardScalingBase::apply_vector_scaling_c_NonConst",
                  dbg_verbosity);
   SmartPtr<Vector> scaled_c = v->MakeNewCopy();
   if( IsValid(scaled_jac_c_space_) && IsValid(scaled_jac_c_space_->RowScaling()) )
   {
      scaled_c->ElementWiseMultiply(*scaled_jac_c_space_->RowScaling());
   }
   else
   {
      DBG_PRINT((1, "Creating copy in apply_vector_scaling_c_NonConst!"));
   }
   return scaled_c;
}

SmartPtr<const Vector> StandardScalingBase::apply_vector_scaling_c(
   const SmartPtr<const Vector>& v
)
{
   DBG_START_METH("NLPScalingObject::apply_vector_scaling_c", dbg_verbosity);
   if( IsValid(scaled_jac_c_space_) && IsValid(scaled_jac_c_space_->RowScaling()) )
   {
      return ConstPtr(apply_vector_scaling_c_NonConst(v));
   }
   else
   {
      return v;
   }
}

SmartPtr<Vector> StandardScalingBase::unapply_vector_scaling_c_NonConst(
   const SmartPtr<const Vector>& v
)
{
   DBG_START_METH("StandardScalingBase::unapply_vector_scaling_c_NonConst",
                  dbg_verbosity);
   SmartPtr<Vector> scaled_c = v->MakeNewCopy();
   if( IsValid(scaled_jac_c_space_) && IsValid(scaled_jac_c_space_->RowScaling()) )
   {
      scaled_c->ElementWiseDivide(*scaled_jac_c_space_->RowScaling());
   }
   else
   {
      DBG_PRINT((1, "Creating copy in unapply_vector_scaling_c_NonConst!"));
   }
   return scaled_c;
}

SmartPtr<const Vector> StandardScalingBase::unapply_vector_scaling_c(
   const SmartPtr<const Vector>& v
)
{
   DBG_START_METH("NLPScalingObject::unapply_vector_scaling_c", dbg_verbosity);
   if( IsValid(scaled_jac_c_space_) && IsValid(scaled_jac_c_space_->RowScaling()) )
   {
      return ConstPtr(unapply_vector_scaling_c_NonConst(v));
   }
   else
   {
      return v;
   }
}

SmartPtr<Vector> StandardScalingBase::apply_vector_scaling_d_NonConst(
   const SmartPtr<const Vector>& v
)
{
   DBG_START_METH("StandardScalingBase::apply_vector_scaling_d_NonConst",
                  dbg_verbosity);
   SmartPtr<Vector> scaled_d = v->MakeNewCopy();
   if( IsValid(scaled_jac_d_space_) && IsValid(scaled_jac_d_space_->RowScaling()) )
   {
      scaled_d->ElementWiseMultiply(*scaled_jac_d_space_->RowScaling());
   }
   else
   {
      DBG_PRINT((1, "Creating copy in apply_vector_scaling_d_NonConst!"));
   }
   return scaled_d;
}

SmartPtr<const Vector> StandardScalingBase::apply_vector_scaling_d(
   const SmartPtr<const Vector>& v
)
{
   DBG_START_METH("NLPScalingObject::apply_vector_scaling_d", dbg_verbosity);
   if( IsValid(scaled_jac_d_space_) && IsValid(scaled_jac_d_space_->RowScaling()) )
   {
      return ConstPtr(apply_vector_scaling_d_NonConst(v));
   }
   else
   {
      return v;
   }
}

SmartPtr<Vector> StandardScalingBase::unapply_vector_scaling_d_NonConst(
   const SmartPtr<const Vector>& v
)
{
   DBG_START_METH("StandardScalingBase::unapply_vector_scaling_d_NonConst",
                  dbg_verbosity);
   SmartPtr<Vector> scaled_d = v->MakeNewCopy();
   if( IsValid(scaled_jac_d_space_) && IsValid(scaled_jac_d_space_->RowScaling()) )
   {
      scaled_d->ElementWiseDivide(*scaled_jac_d_space_->RowScaling());
   }
   else
   {
      DBG_PRINT((1, "Creating copy in unapply_vector_scaling_d_NonConst!"));
   }
   return scaled_d;
}

SmartPtr<const Vector> StandardScalingBase::unapply_vector_scaling_d(
   const SmartPtr<const Vector>& v
)
{
   DBG_START_METH("NLPScalingObject::unapply_vector_scaling_d", dbg_verbosity);
   if( IsValid(scaled_jac_d_space_) && IsValid(scaled_jac_d_space_->RowScaling()) )
   {
      return ConstPtr(unapply_vector_scaling_d_NonConst(v));
   }
   else
   {
      return v;
   }
}

// ToDo: matrix not passed by reference, so setting to NULL doesn't make difference
SmartPtr<const Matrix> StandardScalingBase::apply_jac_c_scaling(
   SmartPtr<const Matrix> matrix
)
{
   DBG_START_METH("NLPScalingObject::apply_jac_c_scaling", dbg_verbosity);
   if( IsValid(scaled_jac_c_space_) )
   {
      SmartPtr<ScaledMatrix> ret = scaled_jac_c_space_->MakeNewScaledMatrix(false);
      ret->SetUnscaledMatrix(matrix);
      return ConstPtr(ret);
   }
   else
   {
      SmartPtr<const Matrix> ret = matrix;
      // matrix = NULL;
      return ret;
   }
}

SmartPtr<const Matrix> StandardScalingBase::apply_jac_d_scaling(
   SmartPtr<const Matrix> matrix
)
{
   DBG_START_METH("NLPScalingObject::apply_jac_d_scaling", dbg_verbosity);
   if( IsValid(scaled_jac_d_space_) )
   {
      SmartPtr<ScaledMatrix> ret = scaled_jac_d_space_->MakeNewScaledMatrix(false);
      ret->SetUnscaledMatrix(matrix);
      return ConstPtr(ret);
   }
   else
   {
      SmartPtr<const Matrix> ret = matrix;
      // matrix = NULL;
      return ret;
   }
}

SmartPtr<const SymMatrix> StandardScalingBase::apply_hessian_scaling(
   SmartPtr<const SymMatrix> matrix
)
{
   DBG_START_METH("NLPScalingObject::apply_hessian_scaling", dbg_verbosity);
   if( IsValid(scaled_h_space_) )
   {
      SmartPtr<SymScaledMatrix> ret = scaled_h_space_->MakeNewSymScaledMatrix(false);
      ret->SetUnscaledMatrix(matrix);
      return ConstPtr(ret);
   }
   else
   {
      SmartPtr<const SymMatrix> ret = matrix;
      // matrix = NULL;
      return ret;
   }
}

bool StandardScalingBase::have_x_scaling()
{
   return IsValid(dx_);
}

bool StandardScalingBase::have_c_scaling()
{
   return (IsValid(scaled_jac_c_space_) && IsValid(scaled_jac_c_space_->RowScaling()));
}

bool StandardScalingBase::have_d_scaling()
{
   return (IsValid(scaled_jac_d_space_) && IsValid(scaled_jac_d_space_->RowScaling()));
}

void NoNLPScalingObject::DetermineScalingParametersImpl(
   const SmartPtr<const VectorSpace>    /*x_space*/,
   const SmartPtr<const VectorSpace>    /*c_space*/,
   const SmartPtr<const VectorSpace>    /*d_space*/,
   const SmartPtr<const MatrixSpace>    /*jac_c_space*/,
   const SmartPtr<const MatrixSpace>    /*jac_d_space*/,
   const SmartPtr<const SymMatrixSpace> /*h_space*/,
   const Matrix&                        /*Px_L*/,
   const Vector&                        /*x_L*/,
   const Matrix&                        /*Px_U*/,
   const Vector&                        /*x_U*/,
   Number&                              df,
   SmartPtr<Vector>&                    dx,
   SmartPtr<Vector>&                    dc,
   SmartPtr<Vector>&                    dd
)
{
   df = 1.;
   dx = NULL;
   dc = NULL;
   dd = NULL;
}

} // namespace Ipopt
